// ¶¨Òå±ê×¼µãÔÆ£¬²¢Ìá¹©»ñÈ¡ºÍ´¦Àí±ê×¼µãÔÆ£¬ÒÔ¼°×ÛºÏ·´¹â°å½øÐÐµãÔÆÆ¥Åä
// Ver 1.0
// »ùÓÚlib_ICP Ver1.3
// 2021-MAR-15


#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <ctime>
#include <cstring>

//#include <eigen3/Eigen/Dense>
#include <Eigen/Dense>

#include "lib_composite_ICP.h"
#include <iostream>

//#define _DEBUG


using namespace Eigen;


// ´ø²ÎÊý¹¹Ôìº¯Êý
point_cloud_type::point_cloud_type(const unsigned size_of_array)
{
	if (size_of_array <= NUM_POINTS_STANDARD_SCAN)
	{
		array_size = size_of_array;
	}
	else
	{
		array_size = NUM_POINTS_STANDARD_SCAN;
	}

	// Ëù±£´æµÄÊý¾ÝµãÊý³õÊ¼»¯Îª0
	num_points = 0;

	// Á¬Ðø²âÁ¿µãµÄ×ÜÊýºÍÆ½¾ù¾àÀëÒ²³õÊ¼»¯Îª0
	num_good_points = 0;
	average_range = 0;
}

void point_cloud_type::set_array_size(const unsigned std_point_num)
{
        array_size = std_point_num;	  
}


unsigned point_cloud_subsampling(const point_cloud_raw_type* point_cloud_raw, point_cloud_type* point_cloud,
	const unsigned sample_ratio, const unsigned bad_range_num_threshold)
{

	// 计算抽样后，将得到多少个点
	// 注意等式右侧可能不能整除，自动取整的效果是，能够保证总是每sample_ratio个原始点得到一个抽样点
	unsigned num_subsample_points = point_cloud_raw->num_points_per_scan / sample_ratio;

	// 确保point_cloud结构体中能够容纳抽样后的点云数据
	if (num_subsample_points > point_cloud->array_size)
	{

	  // point_cloud_type类型的结构体，不足以容纳采样后的点云数据
		printf("Error! point_cloud_type connot hold too many data points specified by sample_ratio.\n");
		return 0;
	}
	else
	{
		point_cloud->num_points = num_subsample_points;
	}

	// 点云的序列号，这里直接沿用原始高密度点云中的序列号
	point_cloud->scan_number = point_cloud_raw->scan_number;

	// 采样后点云数据的角分辨率
	point_cloud->delta_alpha = (double)sample_ratio*point_cloud_raw->angle_increment_rad;

	// 每圈第一个数据点和最后一个数据点的角度值
	double half_delta_alpha = point_cloud->delta_alpha / 2;
	double half_angle_increment = point_cloud_raw->angle_increment_rad / 2;

	point_cloud->alpha_start = point_cloud_raw->angle_start_rad - half_angle_increment + half_delta_alpha;
	point_cloud->alpha_end = point_cloud_raw->angle_end_rad + half_angle_increment - half_delta_alpha;

	// 采样时，是每sample_ratio个原始数据点抽样出一个。
	// 根据sample_ratio动态建立一个数组，保存每sample_ratio个原始数据点中有效的数据点
	double* good_range_array = new double[sample_ratio];

	unsigned raw_data_base_index = 0;
	double min_range_value = 0.0;
	double range_sum = 0.0;
	unsigned num_bad_range, num_good_range, num_average_range;
	double range_temp = 0.0;

	// 清空r和alpha数组
	memset(point_cloud->r, 0, sizeof(point_cloud->r));
	memset(point_cloud->alpha, 0, sizeof(point_cloud->alpha));

	for (unsigned i = 0; i < num_subsample_points; i++)
	{
		// 抽样区间在原始点云中的起始index
		raw_data_base_index = sample_ratio * i;

		min_range_value = MAX_GOOD_RANGE;
		num_good_range = 0;
		num_bad_range = 0;
		num_average_range = 0;
		range_sum = 0.0;


		for (unsigned j = 0; j < sample_ratio; j++)
		{

			range_temp = point_cloud_raw->range[raw_data_base_index + j];

			if (range_temp > MAX_GOOD_RANGE || range_temp < MIN_GOOD_RANGE)
			{
				// 测距距离大于MAX_GOOD_RANGE或小于MIN_GOOD_RANGE的点，为无效测量点，2021.3.19
				num_bad_range++;
			}
			else
			{
				// 记录小于RANGE_MAX的点，找到具有最小测距距离的点
				min_range_value = MIN(min_range_value, range_temp);
				good_range_array[num_good_range++] = range_temp;
			}
		}


		// 计算抽样的结果
		if (num_bad_range > bad_range_num_threshold)   // 2020-7-31，这里进行修改，由大于等于改为大于，因为bad_range_num_threshold可能取0
		{
			// 当每sample_ratio中无效效测量数据的总数大于等于阈值bad_range_num_threshold时，就将抽样后的数据点作为一个无效数据点
			point_cloud->r[i] = 0.0; // 给测量距离赋值0，就表示这是一个无效测量
		}
		else
		{
			for (unsigned j = 0; j < num_good_range; j++)
			{
				if (good_range_array[j] < (min_range_value + 0.05))
				{
					range_sum += good_range_array[j];
					num_average_range++;
				}
			}

			point_cloud->r[i] = range_sum / num_average_range;
		}

		point_cloud->alpha[i] = point_cloud->alpha_start + point_cloud->delta_alpha * i;
	}

	// 释放动态分配空间
	delete[] good_range_array;
	good_range_array = NULL;

	return 1;

}

// 点云预处理，确定点云中各个测量点的间断类型和其法线方向等
void basic_pretreatment(point_cloud_type* point_cloud, double lambda)
{

	// 清空point_cloud中的x,y,gamma,point_type
	memset(point_cloud->x, 0, sizeof(point_cloud->x));
	memset(point_cloud->y, 0, sizeof(point_cloud->y));
	memset(point_cloud->gamma, 0, sizeof(point_cloud->gamma));
	memset(point_cloud->point_type, 0, sizeof(point_cloud->point_type));

	// 一帧点云中的数据点总数
	unsigned total_num_points = point_cloud->num_points;

	// 判断间断的距离阈值的系数
	double distance_threshold_coef = sin(point_cloud->delta_alpha) / sin(lambda - point_cloud->delta_alpha);


	// 找到点云中的所有间断点，给point_type属性赋值。
	/* point_type的取值，与如下几种
		-1，无效测量点
		0，表示连续点
		1，表示起始间断点
		2，表示末尾间断点
		3，表示两侧间断点，即孤立点
	 */

	unsigned current_index = 0;
	unsigned prev_index = 0;
	unsigned next_index = 0;
	double distance, distance_threshold;


	// 先查看一遍所有测量点，找出其中的无效点
	for (current_index = 0; current_index < total_num_points; current_index++)
	{
		if ((point_cloud->r[current_index] < MIN_GOOD_RANGE) || (point_cloud->r[current_index] > MAX_GOOD_RANGE))
		{
			// 测距值不在有效范围内，判断为无效测量点
			point_cloud->point_type[current_index] = -1; //point_type取为-1，表示这是一个无效测量点
			point_cloud->r[current_index] = 0.0;
		}
		else
		{
			// 有效测量点的间断类型先清零
			point_cloud->point_type[current_index] = 0;

			// 对于有效测量点，计算其直角坐标
			point_cloud->x[current_index] = point_cloud->r[current_index] * cos(point_cloud->alpha[current_index]);
			point_cloud->y[current_index] = point_cloud->r[current_index] * sin(point_cloud->alpha[current_index]);
		}
	}

	// 进一步确定有效测量点的间断类型
	for (current_index = 0; current_index < total_num_points; current_index++)
	{
		// 当前点和下一个点的index，注意对于360度激光需要处理一圈首尾衔接问题。
		if (current_index < (total_num_points - 1))
		{
			next_index = current_index + 1;
		}
		else
		{
			// 跨越了180度
			next_index = 0; // 注意这里应该是0，不是1
		}

		// 根据当前点和下一个点的特性，判断其间断点类型。
		// 对于非360度激光，可以沿用360度激光的算法，这是因为，非360度激光每一圈的第一个点和最后一个点，其距离很大，可以保证超过后面的distance_threshold
		if ((point_cloud->point_type[current_index] >= 0) && (point_cloud->point_type[next_index] >= 0))
		{
			// 两个数据点都是有效测量数据
			// 计算当前点与下一个点的距离，以及判断是否间断的距离阈值
			//distance = fabs(point_cloud->r[current_index] - point_cloud->r[next_index]);
			distance = sqrt(point_cloud->r[current_index]* point_cloud->r[current_index] + point_cloud->r[next_index]* point_cloud->r[next_index] 
				- 2* point_cloud->r[current_index]* point_cloud->r[next_index]*cos(point_cloud->delta_alpha));
			distance_threshold = distance_threshold_coef * point_cloud->r[current_index];

			if (distance > distance_threshold)
			{
				if (point_cloud->point_type[current_index] == 0)
				{
					// 当前点是末尾间断
					point_cloud->point_type[current_index] = 2;
				}
				else
				{
					// 当前点是两侧间断
					point_cloud->point_type[current_index] = 3;
				}

				//下一个点是起始间断
				point_cloud->point_type[next_index] = 1;
			}
		}
		else if (point_cloud->point_type[current_index] == -1 && point_cloud->point_type[next_index] >= 0)
		{
			// 当前点是无效测量，但下一个点是有效测量点
			// 下一个点是起始间断点
			point_cloud->point_type[next_index] = 1;
		}
		else if (point_cloud->point_type[current_index] >= 0 && point_cloud->point_type[next_index] == -1)
		{
			// 当前点为有效点，下一个点是无效测量的情况
			if (point_cloud->point_type[current_index] == 0)
			{
				// 当前点为末尾间断点
				point_cloud->point_type[current_index] = 2;
			}
			else
			{
				//当前点为两侧间断点
				point_cloud->point_type[current_index] = 3;
			}
		}
		else
		{
			// 最后这种情况是两个点都为无效点，因为已经将他们标记为-1了，这种情况下不用进一步处理
		}
	}


	// 删除掉数据点数少于MIN_NUM_POINTS_IN_CONTOUR的轮廓，即将其所有点的类型赋值为3（孤立点）
	unsigned num_point_in_contour = 0;
	unsigned continuous_search_start_index = 0;
	unsigned continuous_search_end_index = point_cloud->num_points - 1;
	unsigned contour_start_index = 0;
	unsigned contour_end_index = 0;


	// 在第一个数据点是连续点或结尾点时，需要向前搜索以找到起始点
	if (point_cloud->point_type[0] == 0 || point_cloud->point_type[0] == 2)
	{
		num_point_in_contour = 1; // 轮廓中包含了一个点，即scan的第一个点（index=0）

		// 向前搜索（逆着点云方向）
		for (unsigned i = point_cloud->num_points - 1; i > 0; i--)
		{
			if (point_cloud->point_type[i] == 0)
			{
				num_point_in_contour++;
			}
			else if (point_cloud->point_type[i] == 1)
			{
				// 找到起始点
				num_point_in_contour++;
				contour_start_index = i;
				continuous_search_end_index = MAX(0, i - 1);// 更新连续搜索的终点
				break;
			}
		}
	}

	if (point_cloud->point_type[0] == 0)
	{
		// 还需要向后搜索（顺着点云方向）以找到结束点
		for (unsigned i = 1; i < contour_start_index; i++)
		{
			if (point_cloud->point_type[i] == 0)
			{
				num_point_in_contour++;
			}
			else if (point_cloud->point_type[i] == 2)
			{
				// 找到结束点
				num_point_in_contour++;
				contour_end_index = i;
				continuous_search_start_index = MIN(i + 1, point_cloud->num_points - 1); // 更新连续搜索的起点
				break;
			}
		}

		// 判断轮廓包含的数据点数是否够多
		if (num_point_in_contour < MIN_NUM_POINTS_IN_CONTOUR)
		{
			// 数据点数不够多，将整个轮廓剔除
			for (unsigned i = contour_start_index; i < point_cloud->num_points; i++)
			{
				point_cloud->point_type[i] = 3;
			}
			for (unsigned i = 0; i <= contour_end_index; i++)
			{
				point_cloud->point_type[i] = 3;
			}
		}

	}
	else if (point_cloud->point_type[0] == 2)
	{
		// scan第一个点就是结尾点，不用向后搜索了
		contour_end_index = 0;
		continuous_search_start_index = MIN(1, point_cloud->num_points - 1); // 更新连续搜索的起点

		// 判断轮廓包含的数据点数是否够多
		if (num_point_in_contour < MIN_NUM_POINTS_IN_CONTOUR)
		{
			// 数据点数不够多，将整个轮廓剔除
			for (unsigned i = contour_start_index; i < point_cloud->num_points; i++)
			{
				point_cloud->point_type[i] = 3;
			}
			point_cloud->point_type[0] = 3;
		}
	}


	// 处理剩下的数据点
	num_point_in_contour = 0;
	for (unsigned i = continuous_search_start_index; i <= continuous_search_end_index; i++)
	{
		if (point_cloud->point_type[i] == 1)
		{
			num_point_in_contour = 1;
			contour_start_index = i;
		}
		else if (point_cloud->point_type[i] == 0)
		{
			num_point_in_contour++;
		}
		else if (point_cloud->point_type[i] == 2)
		{
			num_point_in_contour++;
			contour_end_index = i;

			// 判断轮廓中的点数是否够多
			if (num_point_in_contour < MIN_NUM_POINTS_IN_CONTOUR)
			{
				// 数据点数不够多，将整个轮廓剔除
				for (unsigned j = contour_start_index; j <= contour_end_index; j++)
				{
					point_cloud->point_type[j] = 3;
				}
			}
		}
	}


	// 连续点的总数和距离总和
	point_cloud->num_good_points = 0;
	double range_sum = 0;
	double x_sum = 0;
	double y_sum = 0;


	// 逐个点计算法线角度
	double normal_angle_tempt, normal_angle_next_temp, normal_angle_diff_temp, normal_angle_diff;

	for (current_index = 0; current_index < total_num_points; current_index++)
	{
		// 当前点和下一个点的index，注意对于360度激光需要处理一圈首尾衔接问题。
		if (current_index < (total_num_points - 1))
		{
			next_index = current_index + 1;
		}
		else
		{
			// 跨越了180度
			next_index = 0; // 注意这里应该是0，不是1
		}

		// 计算点上的法线方向，注意这里不会计算无效数据点的法线方向
		if (point_cloud->point_type[current_index] == 3)
		{
			// 当前点为两侧间断点，直接根据这个点的坐标计算法线方向
			point_cloud->gamma[current_index] = atan2(-point_cloud->y[current_index], -point_cloud->x[current_index]);
		}
		else if (point_cloud->point_type[current_index] == 1)
		{
			// 当前点为起始间断点，将根据这个点和下一个点的坐标，计算其法线方向
			normal_angle_tempt = atan2(point_cloud->y[current_index] - point_cloud->y[next_index], point_cloud->x[current_index] - point_cloud->x[next_index]) - M_PI_2;
			point_cloud->gamma[current_index] = round_neg_pi_pos_pi(normal_angle_tempt);

			point_cloud->gamma[next_index] = point_cloud->gamma[current_index];
		}
		else if (point_cloud->point_type[current_index] == 0)
		{
			// 当前点为连续点，需要综合两侧的点，计算出法线方向
			// 首先判断该点是否是一圈的第一个点
			if (current_index == 0)
			{
				// 是一圈的第一个点，需要计算其与之前一个点确定的法线
				prev_index = total_num_points - 1;
				normal_angle_tempt = atan2(point_cloud->y[prev_index] - point_cloud->y[current_index], point_cloud->x[prev_index] - point_cloud->x[current_index]) - M_PI_2;

				// 这个法线方向暂存在gamma[current_index]中，后续会对其进行调整
				point_cloud->gamma[current_index] = round_neg_pi_pos_pi(normal_angle_tempt);
			}

			// 计算其与之后一个点确定的法线
			normal_angle_next_temp = atan2(point_cloud->y[current_index] - point_cloud->y[next_index], point_cloud->x[current_index] - point_cloud->x[next_index]) - M_PI_2;
			point_cloud->gamma[next_index] = round_neg_pi_pos_pi(normal_angle_next_temp);

			// 综合这两个法线值，对gamma[current_index]进行调整
			normal_angle_diff_temp = point_cloud->gamma[next_index] - point_cloud->gamma[current_index];
			normal_angle_diff = round_neg_pi_pos_pi(normal_angle_diff_temp);

			normal_angle_tempt = point_cloud->gamma[current_index] + 0.5*normal_angle_diff;
			point_cloud->gamma[current_index] = round_neg_pi_pos_pi(normal_angle_tempt);

			// 在这里增加一个功能，计算出有效测量点的总数、平均距离和重心
			point_cloud->num_good_points++;
			range_sum += point_cloud->r[current_index];
			x_sum += point_cloud->x[current_index];
			y_sum += point_cloud->y[current_index];

		}
		// 当前点为末尾间断的情况下，不用计算，因为已经在其之前的点处，计算了gamma[next_index]

	}

	// 根据统计出的连续点总数，计算良好测量点的平均距离
	if (point_cloud->num_good_points > 0)
	{
		point_cloud->average_range = range_sum / (double)point_cloud->num_good_points;
		point_cloud->mass_center_x = x_sum / (double)point_cloud->num_good_points;
		point_cloud->mass_center_y = y_sum / (double)point_cloud->num_good_points;
	}
	else
	{
		point_cloud->average_range = 0;
		point_cloud->mass_center_x = 0;
		point_cloud->mass_center_y = 0;
	}

}

pose_type pose_transform(const pose_type* inital_pose, const pose_type* transform_pose)
{
  double delta_theta = transform_pose->theta;
  double cos_delta_theta = cos(delta_theta);
  double sin_delta_theta = sin(delta_theta);

  pose_type result_pose;

  result_pose.x = transform_pose->x + inital_pose->x*cos_delta_theta - inital_pose->y*sin_delta_theta;
  result_pose.y = transform_pose->y + inital_pose->x*sin_delta_theta + inital_pose->y*cos_delta_theta;
  result_pose.theta = delta_theta + inital_pose->theta;

  result_pose.theta = atan2(sin(result_pose.theta),cos(result_pose.theta));

  return result_pose;
}

void robust_ekf_pose(pose_type* laser_scanner_pose,double P_element[6],const pose_type ref_map_pose,const double refmap_sigma_array[6],double M_dis_th)
{
  //Ž«ÈëµÄµÚÒ»žöºÍµÚ¶þžö²ÎÊýÒÑŸ­ÊÇk+1|kÊ±¿ÌµÄÎ»×ËŒ°ÆäÐ­·œ²îŸØÕó
  Eigen::Matrix<double,3,1> X_k_1_k;
  Eigen::Matrix<double,3,3> P_k_1_k;
  X_k_1_k << laser_scanner_pose->x,laser_scanner_pose->y,laser_scanner_pose->theta;
  P_k_1_k << P_element[0],P_element[1],P_element[3],
    P_element[1],P_element[2],P_element[4],
    P_element[3],P_element[4],P_element[5];

  //ŒÆËãÂË²šÆ÷ÔöÒæ
  Eigen::Matrix<double,3,3> K_k_1;
  Eigen::Matrix<double,3,3> S_k_1_k;
  Eigen::Matrix<double,3,3> R_k_1_k;

  R_k_1_k << refmap_sigma_array[0],refmap_sigma_array[1],refmap_sigma_array[3],
    refmap_sigma_array[1],refmap_sigma_array[2],refmap_sigma_array[4],
    refmap_sigma_array[3],refmap_sigma_array[4],refmap_sigma_array[5];
  
  S_k_1_k = P_k_1_k + R_k_1_k;
  //K_k_1 = P_k_1_k.transpose()*S_k_1_k.inverse();

  //printf("obs's std is:%f,%f,%f\n",sqrt(S_k_1_k(0,0)), sqrt(S_k_1_k(1,1)), sqrt(S_k_1_k(2,2)));
  //Ð£ÕýÏµÍ³ŒÆËã
  Eigen::Matrix<double,3,1> X_k_1_k_1;
  Eigen::Matrix<double,3,1> y_k_1;
  Eigen::Matrix<double,3,3> P_k_1_k_1;

  y_k_1(0,0) = ref_map_pose.x - X_k_1_k(0,0);
  y_k_1(1,0) = ref_map_pose.y - X_k_1_k(1,0);
  y_k_1(2,0) = ref_map_pose.theta - X_k_1_k(2,0);
  y_k_1(2,0) = atan2(sin(y_k_1(2,0)),cos(y_k_1(2,0)));

  double m_dis = y_k_1.transpose()*S_k_1_k.inverse()*y_k_1;//+
  //printf("m_distance:%f\n",m_dis);//+
  double gama = 0.0;
  double kappa = M_dis_th;
  //printf("kappa:%f\n",kappa);
  if(m_dis <= kappa)//+
    gama = 1.0;
  else
    gama = m_dis / kappa;//+

  //if(gama > 1)
  //printf("original sk1k:%f,%f,%f,%f,%f,%f\n",S_k_1_k(0,0),S_k_1_k(1,0),S_k_1_k(1,1),S_k_1_k(2,0),S_k_1_k(2,1),S_k_1_k(2,2));s
  S_k_1_k = gama * S_k_1_k;//+
  K_k_1 = P_k_1_k.transpose()*S_k_1_k.inverse();//+
  //if(gama > 1)
  //printf("after sk1k:%f,%f,%f\n",S_k_1_k(0,0),S_k_1_k(0,1),S_k_1_k(0,2));
  

  X_k_1_k_1 = X_k_1_k + K_k_1 * y_k_1;
  //P_k_1_k_1 = P_k_1_k - K_k_1 * P_k_1_k;

  Eigen::Matrix<double,3,3> R_k_1_k_;//+
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity();//+
  R_k_1_k_ = (gama - 1.0) * P_k_1_k + gama * R_k_1_k;//+
  if(gama > 1.0)
    P_k_1_k_1 = (I - K_k_1) * P_k_1_k *(I - K_k_1).transpose() + K_k_1 * R_k_1_k_ * K_k_1.transpose();//+
  else
    P_k_1_k_1 = P_k_1_k - K_k_1 * P_k_1_k;

  laser_scanner_pose->x = X_k_1_k_1(0,0);laser_scanner_pose->y = X_k_1_k_1(1,0);laser_scanner_pose->theta = X_k_1_k_1(2,0);
  P_element[0] = P_k_1_k_1(0,0);
  P_element[1] = P_k_1_k_1(1,0);
  P_element[2] = P_k_1_k_1(1,1);
  P_element[3] = P_k_1_k_1(2,0);
  P_element[4] = P_k_1_k_1(2,1);
  P_element[5] = P_k_1_k_1(2,2);
}


// 会对点云和反光板都进行运动补偿
void point_cloud_motion_compensation(point_cloud_type* point_cloud, const pose_type* delta_odo)
{
	// 点云中的数据点总数
	double N = (double)point_cloud->num_points;


	// 激光系中的位姿变化量
	double delta_x = delta_odo->x;
	double delta_y = delta_odo->y;
	double delta_theta = delta_odo->theta;


	// 数据点在激光系中的极坐标和直角坐标
	double point_range, point_alpha, point_x, point_y;

	double angle_temp, gain_temp, delta_x_i, delta_y_i, delta_theta_i;
	double point_x_compensated, point_y_compensated;


	for (unsigned index = 0; index < point_cloud->num_points; index++)
	{
		if (point_cloud->r[index] > 0.15) {
			//CCW mode
                        //angle_temp = delta_theta * (index + 1) / N;
			//gain_temp = (N - index - 1) / N;

                        //CW mode
                        angle_temp = delta_theta * (N - index - 1) / N;//cs16,orinally CW,but output is converted to CCW!!!
		        gain_temp = (double)(index + 1) / N;//cs16,orinally CW,but output is converted to CCW!!! 


			delta_x_i = gain_temp * (delta_x * cos(angle_temp) + delta_y * sin(angle_temp));
			delta_y_i = gain_temp * (-delta_x * sin(angle_temp) + delta_y * cos(angle_temp));
			delta_theta_i = gain_temp * delta_theta;

			// 计算出直角坐标
			point_range = point_cloud->r[index];
			point_alpha = point_cloud->alpha[index];

			point_x = point_range * cos(point_alpha);
			point_y = point_range * sin(point_alpha);

			// 利用直角坐标补偿
			point_x_compensated = (point_x - delta_x_i)*cos(delta_theta_i) + (point_y - delta_y_i)*sin(delta_theta_i);
			point_y_compensated = -(point_x - delta_x_i)*sin(delta_theta_i) + (point_y - delta_y_i)*cos(delta_theta_i);
			point_cloud->x[index] = point_x_compensated;
			point_cloud->y[index] = point_y_compensated;
			point_cloud->r[index] = sqrt(point_x_compensated*point_x_compensated + point_y_compensated * point_y_compensated);
			point_cloud->alpha[index] = atan2(point_y_compensated, point_x_compensated);
		}
	}
	
}

void calcu_cov(const double map[6],const double sig[6],double P[6],const pose_type* map_pose,const pose_type* delta)
{
  //¸ù¾Ýµ±Ç°Î»×ËÏà¶ÔÓÚ×ÓÍ¼µÄÎ»×ËºÍÐ­·½²î£¬ÒÔ¼°×ÓÍ¼µÄÎ»×ËºÍÐ­·½²î£¬¼ÆËãµ±Ç°È«¾ÖÎ»×ËµÄÐ­·½²îÐ­·½²îP_element
  double t1 = -sin(map_pose->theta)*delta->x - cos(map_pose->theta)*delta->y;
  double t2 = cos(map_pose->theta)*delta->x - sin(map_pose->theta)*delta->y;
  Eigen::Matrix<double,3,3> F;
  Eigen::Matrix<double,3,3> M;
  Eigen::Matrix<double,3,3> S;
  Eigen::Matrix<double,3,3> R;
  Eigen::Matrix<double,3,3> result;

  F<<1,0,t1,
    0,1,t2,
    0,0,1;
  M<<map[0],map[1],map[3],
    map[1],map[2],map[4],
    map[3],map[4],map[5];
  R<<cos(map_pose->theta),-sin(map_pose->theta),0,
    sin(map_pose->theta),cos(map_pose->theta),0,
    0,0,1;
  S<<sig[0],sig[1],sig[3],
    sig[1],sig[2],sig[4],
    sig[3],sig[4],sig[5];

  result = F*M*F.transpose() + R*S*R.transpose();

  P[0] = result(0,0) ;
  P[1] = result(1,0);
  P[2] = result(1,1) ;
  P[3] = result(2,0);
  P[4] = result(2,1);
  P[5] = result(2,2) ;  
}
